#!/usr/bin/env python3
import os
import cv2
import numpy as np
import time
import threading
import queue
from dataclasses import dataclass
from typing import Optional
import traceback
# from opt_visualizer import MergedPosePositionVisualizer  # 如果使用优化的可视化器，请取消注释
# from opt_visualizer import RealtimeOptimizedVisualizer   # 如果使用实时优化的可视化器，请取消注释
from image_provider import ImageProvider
from seg_pose import SegPose
from compute_position import ComputePosition  
from utils import get_depth_image
from logger import setup_logger

logger = setup_logger()

@dataclass
class ImageData:
    """图像数据结构"""
    timestamp: str
    readable_timestamp: str
    color_img: np.ndarray
    depth_img: np.ndarray
    color_path: Optional[str] = None

@dataclass
class ModelResult:
    """模型结果数据结构"""
    timestamp: str
    readable_timestamp: str
    color_img: np.ndarray
    depth_img: np.ndarray
    seg_result: any
    pose_result: any
    color_path: Optional[str] = None
    process_time: float = 0.0
    queue_wait_time: float = 0.0  # 新增：队列等待时间

@dataclass
class PositionResult:
    """位置计算结果数据结构"""
    timestamp: str
    readable_timestamp: str
    pose: any
    vector: any
    color_path: Optional[str] = None
    process_time: float = 0.0
    queue_wait_time: float = 0.0  # 新增：队列等待时间

def setup_standard_position(seg_pose,compute_position):
    """设置标准位置 - 独立函数"""

    st_img = cv2.imread("./data/Color/st.jpg")
    st_depth_img = get_depth_image("./data/Color/st.jpg")

    logger.info("==========设置标准位置==========")
    seg_result, pose_result = seg_pose.get_result(st_img)
    if seg_result is None or pose_result is None:
        logger.error("==========没有检测到标准图片的seg结果或pose结果==========")
        return None, None

    st_pose, st_vector = compute_position.set_standard(st_img, st_depth_img, seg_result, pose_result)
    if st_pose is None or st_vector is None:
        logger.error("==========标准位置设定失败==========")
        return None, None
    else:
        logger.info("==========标准位置设定成功==========")
        logger.info(f"标准位置: {st_pose}")
        logger.info(f"标准向量: {st_vector}")
        return st_pose, st_vector
    
class PerformanceMonitor:
    """性能监控类"""
    
    def __init__(self):
        self.lock = threading.Lock()
        self.stats = {
            'model_times': [],
            'position_times': [],
            'viz_times': [],
            'queue_wait_times': {
                'model': [],
                'position': [],
                'viz': []
            },
            'errors': {
                'model': 0,
                'position': 0,
                'viz': 0
            },
            'queue_drops': {  # 新增：记录队列丢弃的数据数量
                'image': 0,
                'model': 0,
                'position': 0
            }
        }
    
    def record_time(self, category: str, time_ms: float):
        """记录处理时间"""
        with self.lock:
            if category in self.stats:
                self.stats[category].append(time_ms)
    
    def record_queue_wait(self, stage: str, wait_time_ms: float):
        """记录队列等待时间"""
        with self.lock:
            if stage in self.stats['queue_wait_times']:
                self.stats['queue_wait_times'][stage].append(wait_time_ms)
    
    def record_error(self, category: str):
        """记录错误"""
        with self.lock:
            if category in self.stats['errors']:
                self.stats['errors'][category] += 1
    
    def record_queue_drop(self, queue_name: str, drop_count: int):
        """记录队列丢弃数量"""
        with self.lock:
            if queue_name in self.stats['queue_drops']:
                self.stats['queue_drops'][queue_name] += drop_count
    
    def get_summary(self):
        """获取统计摘要"""
        with self.lock:
            summary = {}
            for key, times in self.stats.items():
                if key == 'queue_wait_times':
                    summary[key] = {}
                    for stage, wait_times in times.items():
                        if wait_times:
                            summary[key][stage] = {
                                'avg': sum(wait_times) / len(wait_times),
                                'max': max(wait_times),
                                'min': min(wait_times),
                                'count': len(wait_times)
                            }
                elif key in ['errors', 'queue_drops']:
                    summary[key] = times
                elif times:  # 处理时间列表
                    summary[key] = {
                        'avg': sum(times) / len(times),
                        'max': max(times),
                        'min': min(times),
                        'count': len(times)
                    }
            return summary

class MultiThreadProcessor:
    """集成实时可视化的优化处理器"""

    def __init__(self, st_pose, st_vector, seg_pose_instance, compute_position_instance, 
                 max_queue_size: int = 10, realtime_viz: bool = True, target_fps: int = 15):
        self.st_pose = st_pose
        self.st_vector = st_vector
        
        # 队列 - 实时模式下减小队列大小
        queue_size = max(3, max_queue_size // 2) if realtime_viz else max_queue_size
        self.image_queue = queue.Queue(maxsize=queue_size)
        self.model_queue = queue.Queue(maxsize=queue_size)
        self.position_queue = queue.Queue(maxsize=queue_size)
        
        # 处理器
        self.seg_pose = seg_pose_instance
        self.compute_position = compute_position_instance
        
        # 使用实时优化的可视化器
        self.realtime_viz = realtime_viz
        self.target_fps = target_fps
        # self.visualizer = RealtimeOptimizedVisualizer(
        #     realtime_mode=realtime_viz,
        #     target_fps=target_fps,
        #     enable_display=True
        # )
        
        # 性能监控
        self.performance_monitor = PerformanceMonitor()
        
        # 线程控制
        self.stop_event = threading.Event()
        self.threads = []
        
        # 实时可视化统计
        self.viz_frame_count = 0
        self.viz_start_time = time.time()
        self.last_viz_stats_time = time.time()
        
        # 统计信息
        self.stats = {
            'images_processed': 0,
            'models_processed': 0,
            'positions_processed': 0,
            'visualizations_processed': 0,
            'start_time': time.time(),
            'last_stats_time': time.time()
        }
        self.stats_lock = threading.Lock()
        
        logger.info(f"优化处理器初始化完成，实时可视化: {realtime_viz}, 目标FPS: {target_fps}")

    def _visualization_worker_optimized(self):
        """优化的可视化工作线程"""
        logger.info("优化可视化线程已启动")
        
        frame_skip_count = 0
        max_frame_skip = 2 if self.realtime_viz else 0  # 实时模式允许跳帧
        
        while not self.stop_event.is_set():
            try:
                # 记录队列等待开始时间
                wait_start = time.time()
                position_result = self.position_queue.get(timeout=1.0)
                wait_time = (time.time() - wait_start) * 1000
                
                # 实时模式下的智能跳帧逻辑
                if self.realtime_viz and not self.position_queue.empty():
                    queue_size = self.position_queue.qsize()
                    if queue_size > 2:  # 队列积压严重时跳帧
                        frame_skip_count += 1
                        if frame_skip_count <= max_frame_skip:
                            logger.debug(f"跳帧 {frame_skip_count}, 队列积压: {queue_size}")
                            continue
                        else:
                            frame_skip_count = 0  # 重置跳帧计数
                
                # 记录队列等待时间
                self.performance_monitor.record_queue_wait('viz', wait_time)
                
                viz_start = time.time()
                
                # 使用实时优化的可视化
                merged_image, frame_time = self.visualizer.visualize_merged_realtime(
                    position_result.pose,
                    self.st_pose,
                    self.st_vector,
                    position_result.vector,
                    position_result.color_path,
                    timestamp=position_result.readable_timestamp,
                    display=True,
                    save=not self.realtime_viz  # 实时模式不保存
                )
                
                viz_time = (time.time() - viz_start) * 1000
                
                # 记录处理时间
                self.performance_monitor.record_time('viz_times', viz_time)
                
                with self.stats_lock:
                    self.stats['visualizations_processed'] += 1
                
                self.viz_frame_count += 1
                
                # 实时可视化性能监控
                current_time = time.time()
                if current_time - self.last_viz_stats_time > 3.0:  # 每3秒输出一次
                    elapsed = current_time - self.viz_start_time
                    if elapsed > 0:
                        actual_fps = self.viz_frame_count / elapsed
                        viz_stats = self.visualizer.get_performance_stats()
                        
                        logger.info(f"📺 可视化性能: 实际FPS={actual_fps:.1f}, "
                                   f"目标FPS={self.target_fps}, "
                                   f"平均帧时间={viz_stats.get('avg_frame_time_ms', 0):.1f}ms, "
                                   f"队列等待={wait_time:.1f}ms")
                        
                        # 动态调整策略
                        if self.realtime_viz and actual_fps < self.target_fps * 0.8:
                            logger.warning(f"⚠️  可视化FPS低于目标的80%，考虑降低分辨率或启用更多跳帧")
                    
                    self.last_viz_stats_time = current_time
                
                logger.debug(f"可视化完成: {position_result.readable_timestamp}, "
                           f"处理耗时: {viz_time:.2f}ms, 队列等待: {wait_time:.2f}ms")
                
            except queue.Empty:
                continue
            except Exception as e:
                logger.error(f"可视化错误: {e}")
                logger.error(f"错误详情: {traceback.format_exc()}")
                self.performance_monitor.record_error('viz')
        
        logger.info("优化可视化线程已退出")

    def _performance_monitor_worker_enhanced(self):
        """增强的性能监控线程"""
        logger.info("增强性能监控线程已启动")
        
        while not self.stop_event.is_set():
            time.sleep(3.0)
            
            with self.stats_lock:
                current_time = time.time()
                elapsed_time = current_time - self.stats['start_time']
                
                if elapsed_time > 0:
                    fps = self.stats['images_processed'] / elapsed_time
                    perf_summary = self.performance_monitor.get_summary()
                    
                    logger.info("=== 增强性能诊断 ===")
                    logger.info(f"🔄 处理进度: 图像{self.stats['images_processed']} → "
                               f"模型{self.stats['models_processed']} → "
                               f"位置{self.stats['positions_processed']} → "
                               f"可视化{self.stats['visualizations_processed']}")
                    logger.info(f"📊 平均输入FPS: {fps:.2f}")
                    
                    # 获取实时可视化统计
                    viz_stats = self.visualizer.get_performance_stats()
                    if viz_stats:
                        logger.info(f"📺 可视化统计: 估计FPS={viz_stats.get('estimated_fps', 0):.1f}, "
                                   f"平均帧时间={viz_stats.get('avg_frame_time_ms', 0):.1f}ms")
                    
                    # 处理时间统计
                    for stage, key in [('模型处理', 'model_times'), ('位置计算', 'position_times'), ('可视化处理', 'viz_times')]:
                        if key in perf_summary:
                            stats = perf_summary[key]
                            logger.info(f"⏱️  {stage}: 平均{stats['avg']:.1f}ms, "
                                       f"最大{stats['max']:.1f}ms, 最小{stats['min']:.1f}ms")
                    
                    # 队列等待时间统计
                    if 'queue_wait_times' in perf_summary:
                        wait_stats = perf_summary['queue_wait_times']
                        for stage, stats in wait_stats.items():
                            if stats:
                                logger.info(f"⏳ {stage}队列等待: 平均{stats['avg']:.1f}ms, 最大{stats['max']:.1f}ms")
                    
                    # 瓶颈分析和建议
                    model_backlog = self.model_queue.qsize()
                    position_backlog = self.position_queue.qsize()
                    image_backlog = self.image_queue.qsize()
                    
                    logger.info(f"📦 队列状态: 图像{image_backlog}, 模型{model_backlog}, 位置{position_backlog}")
                    
                    # 智能瓶颈分析
                    if model_backlog > 3:
                        logger.warning("🚨 位置计算可能是瓶颈！建议优化位置算法")
                    elif position_backlog > 3:
                        if self.realtime_viz:
                            logger.warning("🚨 可视化是瓶颈！已启用实时优化，考虑降低目标FPS")
                        else:
                            logger.warning("🚨 可视化是瓶颈！建议启用实时模式 (realtime_viz=True)")
                    elif image_backlog == 0 and model_backlog == 0 and position_backlog == 0:
                        logger.info("✅ 流水线运行完美，无瓶颈")
                    
                    # 队列丢弃统计
                    if 'queue_drops' in perf_summary:
                        drops = perf_summary['queue_drops']
                        total_drops = sum(drops.values())
                        if total_drops > 0:
                            logger.warning(f"🗑️  队列丢弃: 总计{total_drops} "
                                          f"(图像{drops['image']}, 模型{drops['model']}, 位置{drops['position']})")
                            
                            # 丢弃率分析
                            drop_rate = total_drops / max(1, self.stats['images_processed']) * 100
                            if drop_rate > 10:
                                logger.warning(f"⚠️  丢弃率过高: {drop_rate:.1f}%，建议减少输入速率或优化算法")
                    
                    logger.info("========================")
        
        logger.info("增强性能监控线程已退出")

    def start(self):
        """启动所有线程"""
        # 模型处理线程
        model_thread = threading.Thread(target=self._model_worker, name="ModelWorker")
        model_thread.start()
        self.threads.append(model_thread)
        
        # 位置计算线程
        position_thread = threading.Thread(target=self._position_worker, name="PositionWorker")
        position_thread.start()
        self.threads.append(position_thread)
        
        # 优化的可视化线程
        viz_thread = threading.Thread(target=self._visualization_worker_optimized, name="OptimizedVizWorker")
        viz_thread.start()
        self.threads.append(viz_thread)
        
        # 增强的性能监控线程
        monitor_thread = threading.Thread(target=self._performance_monitor_worker_enhanced, name="EnhancedMonitorWorker")
        monitor_thread.start()
        self.threads.append(monitor_thread)
        
        logger.info("所有优化线程已启动")

    def stop(self):
        """停止所有线程"""
        logger.info("正在停止所有线程...")
        self.stop_event.set()
        
        # 清理可视化器
        if hasattr(self, 'visualizer'):
            self.visualizer.cleanup()
        
        for thread in self.threads:
            thread.join(timeout=5.0)
            if thread.is_alive():
                logger.warning(f"线程 {thread.name} 未能在超时时间内停止")
        
        logger.info("所有线程已停止")

    # 保持其他方法不变，只需要继承或复制原有的方法
    def _clear_queue_keep_last(self, target_queue: queue.Queue, queue_name: str) -> int:
        """清空队列只保留最后一个元素"""
        dropped_count = 0
        last_item = None
        
        while True:
            try:
                item = target_queue.get_nowait()
                if last_item is not None:
                    dropped_count += 1
                last_item = item
            except queue.Empty:
                break
        
        if last_item is not None:
            try:
                target_queue.put_nowait(last_item)
            except queue.Full:
                logger.error(f"重新放入{queue_name}队列时发生异常")
                dropped_count += 1
        
        if dropped_count > 0:
            logger.warning(f"{queue_name}队列已满，丢弃了{dropped_count}个旧元素，保留最新元素")
            self.performance_monitor.record_queue_drop(queue_name.lower(), dropped_count)
        
        return dropped_count
    
    def _safe_put(self, target_queue: queue.Queue, item, queue_name: str) -> bool:
        """安全地向队列添加元素"""
        try:
            target_queue.put_nowait(item)
            return True
        except queue.Full:
            self._clear_queue_keep_last(target_queue, queue_name)
            try:
                target_queue.put_nowait(item)
                return True
            except queue.Full:
                logger.error(f"清空{queue_name}队列后仍然无法添加元素")
                return False

    def add_image(self, image_data: ImageData) -> bool:
        """添加图像到处理队列"""
        success = self._safe_put(self.image_queue, image_data, "Image")
        if success:
            with self.stats_lock:
                self.stats['images_processed'] += 1
        return success

    # 复制原有的 _model_worker 和 _position_worker 方法
    def _model_worker(self):
        """模型处理工作线程"""
        logger.info("模型处理线程已启动")
        
        while not self.stop_event.is_set():
            try:
                wait_start = time.time()
                image_data = self.image_queue.get(timeout=1.0)
                wait_time = (time.time() - wait_start) * 1000
                
                self.performance_monitor.record_queue_wait('model', wait_time)
                
                start_time = time.time()
                seg_result, pose_result = self.seg_pose.get_result(image_data.color_img)
                process_time = (time.time() - start_time) * 1000
                
                self.performance_monitor.record_time('model_times', process_time)
                
                model_result = ModelResult(
                    timestamp=image_data.timestamp,
                    readable_timestamp=image_data.readable_timestamp,
                    color_img=image_data.color_img,
                    depth_img=image_data.depth_img,
                    seg_result=seg_result,
                    pose_result=pose_result,
                    color_path=image_data.color_path,
                    process_time=process_time,
                    queue_wait_time=wait_time
                )
                
                success = self._safe_put(self.model_queue, model_result, "Model")
                if success:
                    with self.stats_lock:
                        self.stats['models_processed'] += 1
                    
                    logger.debug(f"模型处理完成: {image_data.readable_timestamp}, "
                               f"处理耗时: {process_time:.2f}ms, 队列等待: {wait_time:.2f}ms")
                
            except queue.Empty:
                continue
            except Exception as e:
                logger.error(f"模型处理错误: {e}")
                logger.error(f"错误详情: {traceback.format_exc()}")
                self.performance_monitor.record_error('model')
        
        logger.info("模型处理线程已退出")

    def _position_worker(self):
        """位置计算工作线程"""
        logger.info("位置计算线程已启动")
        
        while not self.stop_event.is_set():
            try:
                wait_start = time.time()
                model_result = self.model_queue.get(timeout=1.0)
                wait_time = (time.time() - wait_start) * 1000
                
                self.performance_monitor.record_queue_wait('position', wait_time)
                
                logger.debug(f"位置计算开始: {model_result.readable_timestamp}")
                
                start_time = time.time()
                pose, vector = self.compute_position.process_image_pair(
                    model_result.readable_timestamp,
                    model_result.color_img,
                    model_result.depth_img,
                    model_result.seg_result,
                    model_result.pose_result
                )
                process_time = (time.time() - start_time) * 1000
                
                self.performance_monitor.record_time('position_times', process_time)
                
                position_result = PositionResult(
                    timestamp=model_result.timestamp,
                    readable_timestamp=model_result.readable_timestamp,
                    pose=pose,
                    vector=vector,
                    color_path=model_result.color_path,
                    process_time=process_time,
                    queue_wait_time=wait_time
                )
                
                success = self._safe_put(self.position_queue, position_result, "Position")
                if success:
                    with self.stats_lock:
                        self.stats['positions_processed'] += 1
                    
                    logger.info(f"位置计算完成: {model_result.readable_timestamp}, "
                              f"处理耗时: {process_time:.2f}ms, 队列等待: {wait_time:.2f}ms")
                    logger.info(f"Pose: {pose}, Vector: {vector}")
                
            except queue.Empty:
                continue
            except Exception as e:
                logger.error(f"位置计算错误: {e}")
                logger.error(f"错误详情: {traceback.format_exc()}")
                self.performance_monitor.record_error('position')
        
        logger.info("位置计算线程已退出")


# 修改主函数
def main_optimized():
    """优化的主函数"""
    # seg_pose = SegPose()
    # compute_position = ComputePosition()
    
    # 设置标准位置
    # st_pose, st_vector = setup_standard_position(seg_pose, compute_position)
    # if st_pose is None or st_vector is None:
    #     logger.error("标准位置设定失败，程序退出")
    #     return

    # 配置参数
    from_folder = False
    max_queue_size = 3  # 实时模式下使用更小的队列
    realtime_viz = True  # 启用实时可视化模式
    target_fps = 15  # 目标FPS，根据您的硬件调整
    
    # 创建图像提供器
    image_provider = ImageProvider(from_folder=from_folder)

    # 创建优化的处理器
    # processor = MultiThreadProcessor(
    #     st_pose, st_vector, seg_pose, compute_position, 
    #     max_queue_size, realtime_viz, target_fps
    # )
    
    try:
        # processor.start()
        
        logger.info("开始处理图像（实时优化模式）...")
        
        while True:
            image_data = image_provider.get_next_image()
            if image_data is None:
                logger.info("所有图像读取完成")
                break
            
            # success = processor.add_image(image_data)
            # if not success:
            #     time.sleep(0.005)  # 实时模式下更短的等待时间
        
        # 等待队列处理完成
        logger.info("等待队列处理完成...")
        # while (not processor.image_queue.empty() or 
        #        not processor.model_queue.empty() or 
        #        not processor.position_queue.empty()):
        #     time.sleep(0.5)
        #     logger.info(f"等待中... 队列状态: 图像{processor.image_queue.qsize()}, "
        #                f"模型{processor.model_queue.qsize()}, 位置{processor.position_queue.qsize()}")
        
        logger.info("所有任务处理完成")
        
    except KeyboardInterrupt:
        logger.info("收到中断信号，正在关闭...")
    except Exception as e:
        logger.error(f"程序错误: {e}")
        logger.error(f"错误详情: {traceback.format_exc()}")
    finally:
        # processor.stop()
        image_provider.close()
        
        # 输出最终统计
        # with processor.stats_lock:
        #     total_time = time.time() - processor.stats['start_time']
        #     logger.info("==========优化程序结束==========")
        #     logger.info(f"总共处理图像: {processor.stats['images_processed']}")
        #     logger.info(f"总共可视化: {processor.stats['visualizations_processed']}")
        #     if processor.stats['images_processed'] > 0:
        #         avg_fps = processor.stats['images_processed'] / total_time
        #         logger.info(f"平均输入帧率: {avg_fps:.2f} FPS")
            
        #     # 可视化性能摘要
        #     viz_stats = processor.visualizer.get_performance_stats()
            # if viz_stats:
            #     logger.info(f"可视化平均FPS: {viz_stats['estimated_fps']:.2f}")
            #     logger.info(f"可视化平均帧时间: {viz_stats['avg_frame_time_ms']:.1f}ms")
            
            # logger.info(f"总运行时间: {total_time:.2f} 秒")


if __name__ == "__main__":
    main_optimized()

# class DiagnosticMultiThreadProcessor:
#     """带诊断功能的多线程处理器"""
    
#     def __init__(self, st_pose, st_vector,seg_pose_instance, compute_position_instance, max_queue_size: int = 10):
#         self.st_pose = st_pose
#         self.st_vector = st_vector
        
#         # 队列
#         self.image_queue = queue.Queue(maxsize=max_queue_size)
#         self.model_queue = queue.Queue(maxsize=max_queue_size)
#         self.position_queue = queue.Queue(maxsize=max_queue_size)
        
#         # 处理器 - 使用传入的已初始化实例
#         self.seg_pose = seg_pose_instance
#         self.compute_position = compute_position_instance  # 使用已经设置好标准位置的实例
#         # self.position_visualizer = OptimizedPositionVisualizer()
#         # self.pose_visualizer = OptimizedPoseVisualizer()
#         self.merge_visualizer = MergedPosePositionVisualizer()
#         # 性能监控
#         self.performance_monitor = PerformanceMonitor()
        
#         # 线程控制
#         self.stop_event = threading.Event()
#         self.threads = []
        
#         # 统计信息
#         self.stats = {
#             'images_processed': 0,
#             'models_processed': 0,
#             'positions_processed': 0,
#             'visualizations_processed': 0,
#             'start_time': time.time(),
#             'last_stats_time': time.time()
#         }
#         self.stats_lock = threading.Lock()
    
#     def _clear_queue_keep_last(self, target_queue: queue.Queue, queue_name: str) -> int:
#         """清空队列只保留最后一个元素
        
#         Args:
#             target_queue: 要清理的队列
#             queue_name: 队列名称（用于日志）
            
#         Returns:
#             int: 被丢弃的元素数量
#         """
#         dropped_count = 0
#         last_item = None
        
#         # 取出所有元素，只保留最后一个
#         while True:
#             try:
#                 item = target_queue.get_nowait()
#                 if last_item is not None:
#                     dropped_count += 1
#                 last_item = item
#             except queue.Empty:
#                 break
        
#         # 将最后一个元素放回队列
#         if last_item is not None:
#             try:
#                 target_queue.put_nowait(last_item)
#             except queue.Full:
#                 # 这种情况理论上不应该发生，因为我们刚清空了队列
#                 logger.error(f"重新放入{queue_name}队列时发生异常")
#                 dropped_count += 1
        
#         if dropped_count > 0:
#             logger.warning(f"{queue_name}队列已满，丢弃了{dropped_count}个旧元素，保留最新元素")
#             self.performance_monitor.record_queue_drop(queue_name.lower(), dropped_count)
        
#         return dropped_count
    
#     def _safe_put(self, target_queue: queue.Queue, item, queue_name: str) -> bool:
#         """安全地向队列添加元素，队列满时清空并只保留最后一个
        
#         Args:
#             target_queue: 目标队列
#             item: 要添加的元素
#             queue_name: 队列名称（用于日志）
            
#         Returns:
#             bool: 是否成功添加
#         """
#         try:
#             target_queue.put_nowait(item)
#             return True
#         except queue.Full:
#             # 队列满了，清空并只保留最后一个元素
#             self._clear_queue_keep_last(target_queue, queue_name)
            
#             # 重新尝试添加新元素
#             try:
#                 target_queue.put_nowait(item)
#                 return True
#             except queue.Full:
#                 logger.error(f"清空{queue_name}队列后仍然无法添加元素")
#                 return False
    
#     def start(self):
#         """启动所有线程"""
#         # 模型处理线程
#         model_thread = threading.Thread(target=self._model_worker, name="ModelWorker")
#         model_thread.start()
#         self.threads.append(model_thread)
        
#         # 位置计算线程
#         position_thread = threading.Thread(target=self._position_worker, name="PositionWorker")
#         position_thread.start()
#         self.threads.append(position_thread)
        
#         # 可视化线程
#         viz_thread = threading.Thread(target=self._visualization_worker, name="VizWorker")
#         viz_thread.start()
#         self.threads.append(viz_thread)
        
#         # 性能监控线程
#         monitor_thread = threading.Thread(target=self._performance_monitor_worker, name="MonitorWorker")
#         monitor_thread.start()
#         self.threads.append(monitor_thread)
        
#         logger.info("所有处理线程已启动")
    
#     def stop(self):
#         """停止所有线程"""
#         logger.info("正在停止所有线程...")
#         self.stop_event.set()
        
#         for thread in self.threads:
#             thread.join(timeout=5.0)
#             if thread.is_alive():
#                 logger.warning(f"线程 {thread.name} 未能在超时时间内停止")
        
#         logger.info("所有线程已停止")
    
#     def add_image(self, image_data: ImageData) -> bool:
#         """添加图像到处理队列
        
#         Args:
#             image_data: 待添加的图像数据
            
#         Returns:
#             bool: 是否成功添加
#         """
#         success = self._safe_put(self.image_queue, image_data, "Image")
#         if success:
#             with self.stats_lock:
#                 self.stats['images_processed'] += 1
#         return success
    
#     def _model_worker(self):
#         """模型处理工作线程"""
#         logger.info("模型处理线程已启动")
        
#         while not self.stop_event.is_set():
#             try:
#                 # 记录队列等待开始时间
#                 wait_start = time.time()
#                 image_data = self.image_queue.get(timeout=1.0)
#                 wait_time = (time.time() - wait_start) * 1000
                
#                 # 记录队列等待时间
#                 self.performance_monitor.record_queue_wait('model', wait_time)
                
#                 start_time = time.time()
#                 seg_result, pose_result = self.seg_pose.get_result(image_data.color_img)
#                 process_time = (time.time() - start_time) * 1000
                
#                 # 记录处理时间
#                 self.performance_monitor.record_time('model_times', process_time)
                
#                 model_result = ModelResult(
#                     timestamp=image_data.timestamp,
#                     readable_timestamp=image_data.readable_timestamp,
#                     color_img=image_data.color_img,
#                     depth_img=image_data.depth_img,
#                     seg_result=seg_result,
#                     pose_result=pose_result,
#                     color_path=image_data.color_path,
#                     process_time=process_time,
#                     queue_wait_time=wait_time
#                 )
                
#                 # 使用安全的put方法
#                 success = self._safe_put(self.model_queue, model_result, "Model")
#                 if success:
#                     with self.stats_lock:
#                         self.stats['models_processed'] += 1
                    
#                     logger.debug(f"模型处理完成: {image_data.readable_timestamp}, "
#                                f"处理耗时: {process_time:.2f}ms, 队列等待: {wait_time:.2f}ms")
                
#             except queue.Empty:
#                 continue
#             except Exception as e:
#                 logger.error(f"模型处理错误: {e}")
#                 logger.error(f"错误详情: {traceback.format_exc()}")
#                 self.performance_monitor.record_error('model')
        
#         logger.info("模型处理线程已退出")
    
#     def _position_worker(self):
#         """位置计算工作线程 - 增强诊断"""
#         logger.info("位置计算线程已启动")
        
#         while not self.stop_event.is_set():
#             try:
#                 # 记录队列等待开始时间
#                 wait_start = time.time()
#                 model_result = self.model_queue.get(timeout=1.0)
#                 wait_time = (time.time() - wait_start) * 1000
                
#                 # 记录队列等待时间
#                 self.performance_monitor.record_queue_wait('position', wait_time)
                
#                 logger.debug(f"位置计算开始: {model_result.readable_timestamp}")
                
#                 start_time = time.time()
                
#                 # 详细记录位置计算的各个步骤
#                 step1_start = time.time()
#                 pose, vector = self.compute_position.process_image_pair(
#                     model_result.readable_timestamp,
#                     model_result.color_img,
#                     model_result.depth_img,
#                     model_result.seg_result,
#                     model_result.pose_result
#                 )
#                 step1_time = (time.time() - step1_start) * 1000
                
#                 process_time = (time.time() - start_time) * 1000
                
#                 # 记录处理时间
#                 self.performance_monitor.record_time('position_times', process_time)
                
#                 position_result = PositionResult(
#                     timestamp=model_result.timestamp,
#                     readable_timestamp=model_result.readable_timestamp,
#                     pose=pose,
#                     vector=vector,
#                     color_path=model_result.color_path,
#                     process_time=process_time,
#                     queue_wait_time=wait_time
#                 )
                
#                 # 使用安全的put方法
#                 success = self._safe_put(self.position_queue, position_result, "Position")
#                 if success:
#                     with self.stats_lock:
#                         self.stats['positions_processed'] += 1
                    
#                     logger.info(f"位置计算完成: {model_result.readable_timestamp}, "
#                               f"处理耗时: {process_time:.2f}ms, 队列等待: {wait_time:.2f}ms, "
#                               f"内部计算: {step1_time:.2f}ms")
#                     logger.info(f"Pose: {pose}, Vector: {vector}")
                
#             except queue.Empty:
#                 continue
#             except Exception as e:
#                 logger.error(f"位置计算错误: {e}")
#                 logger.error(f"错误详情: {traceback.format_exc()}")
#                 self.performance_monitor.record_error('position')
        
#         logger.info("位置计算线程已退出")
    
#     def _visualization_worker(self):
#         """可视化工作线程 - 增强诊断"""
#         logger.info("可视化线程已启动")
        
#         while not self.stop_event.is_set():
#             try:
#                 # 记录队列等待开始时间
#                 wait_start = time.time()
#                 position_result = self.position_queue.get(timeout=1.0)
#                 wait_time = (time.time() - wait_start) * 1000
                
#                 # 记录队列等待时间
#                 self.performance_monitor.record_queue_wait('viz', wait_time)
                
#                 start_time = time.time()
                
#                 if position_result.color_path:
#                     self.merge_visualizer.visualize_merged(position_result.pose,self.st_pose,
#                                                            self.st_vector,position_result.vector,position_result.color_path)
                
#                 process_time = (time.time() - start_time) * 1000
                
#                 # 记录处理时间
#                 self.performance_monitor.record_time('viz_times', process_time)
                
#                 with self.stats_lock:
#                     self.stats['visualizations_processed'] += 1
                
#                 logger.debug(f"可视化完成: {position_result.readable_timestamp}, "
#                            f"处理耗时: {process_time:.2f}ms, 队列等待: {wait_time:.2f}ms")
                
#             except queue.Empty:
#                 continue
#             except Exception as e:
#                 logger.error(f"可视化错误: {e}")
#                 logger.error(f"错误详情: {traceback.format_exc()}")
#                 self.performance_monitor.record_error('viz')
        
#         logger.info("可视化线程已退出")
    
#     def _performance_monitor_worker(self):
#         """性能监控线程"""
#         logger.info("性能监控线程已启动")
        
#         while not self.stop_event.is_set():
#             time.sleep(3.0)  # 每3秒输出一次详细统计
            
#             with self.stats_lock:
#                 current_time = time.time()
#                 elapsed_time = current_time - self.stats['start_time']
#                 period_time = current_time - self.stats['last_stats_time']
#                 self.stats['last_stats_time'] = current_time
                
#                 if elapsed_time > 0:
#                     fps = self.stats['images_processed'] / elapsed_time
                    
#                     # 获取性能统计
#                     perf_summary = self.performance_monitor.get_summary()
                    
#                     logger.info("=== 详细性能诊断 ===")
#                     logger.info(f"处理进度: 图像{self.stats['images_processed']} → 模型{self.stats['models_processed']} → 位置{self.stats['positions_processed']} → 可视化{self.stats['visualizations_processed']}")
#                     logger.info(f"平均FPS: {fps:.2f}")
                    
#                     # 处理时间统计
#                     if 'model_times' in perf_summary:
#                         model_stats = perf_summary['model_times']
#                         logger.info(f"模型处理: 平均{model_stats['avg']:.2f}ms, 最大{model_stats['max']:.2f}ms, 最小{model_stats['min']:.2f}ms")
                    
#                     if 'position_times' in perf_summary:
#                         pos_stats = perf_summary['position_times']
#                         logger.info(f"位置计算: 平均{pos_stats['avg']:.2f}ms, 最大{pos_stats['max']:.2f}ms, 最小{pos_stats['min']:.2f}ms")
                    
#                     if 'viz_times' in perf_summary:
#                         viz_stats = perf_summary['viz_times']
#                         logger.info(f"可视化处理: 平均{viz_stats['avg']:.2f}ms, 最大{viz_stats['max']:.2f}ms, 最小{viz_stats['min']:.2f}ms")
                    
#                     # 队列等待时间统计
#                     if 'queue_wait_times' in perf_summary:
#                         wait_stats = perf_summary['queue_wait_times']
#                         for stage, stats in wait_stats.items():
#                             if stats:
#                                 logger.info(f"{stage}队列等待: 平均{stats['avg']:.2f}ms, 最大{stats['max']:.2f}ms")
                    
#                     # 队列丢弃统计
#                     if 'queue_drops' in perf_summary:
#                         drops = perf_summary['queue_drops']
#                         total_drops = sum(drops.values())
#                         if total_drops > 0:
#                             logger.warning(f"队列丢弃统计: 图像{drops['image']}, 模型{drops['model']}, 位置{drops['position']}")
                    
#                     # 错误统计
#                     if 'errors' in perf_summary:
#                         errors = perf_summary['errors']
#                         total_errors = sum(errors.values())
#                         if total_errors > 0:
#                             logger.warning(f"错误统计: 模型{errors['model']}, 位置{errors['position']}, 可视化{errors['viz']}")
                    
#                     # 队列状态
#                     logger.info(f"队列状态: 图像{self.image_queue.qsize()}, 模型{self.model_queue.qsize()}, 位置{self.position_queue.qsize()}")
                    
#                     # 瓶颈分析
#                     model_backlog = self.model_queue.qsize()
#                     position_backlog = self.position_queue.qsize()
                    
#                     if model_backlog > 5:
#                         logger.warning("⚠️  位置计算可能是瓶颈！模型结果队列积压过多")
#                     elif position_backlog > 5:
#                         logger.warning("⚠️  可视化可能是瓶颈！位置结果队列积压过多")
#                     elif self.image_queue.qsize() == 0 and model_backlog == 0:
#                         logger.info("✅ 流水线运行良好，无明显瓶颈")
                    
#                     logger.info("==================")
        
#         logger.info("性能监控线程已退出")

# # 修改主函数以使用诊断处理器
# def setup_standard_position(seg_pose,compute_position):
#     """设置标准位置 - 独立函数"""

#     st_img = cv2.imread("./data/Color/st.jpg")
#     st_depth_img = get_depth_image("./data/Color/st.jpg")

#     logger.info("==========设置标准位置==========")
#     seg_result, pose_result = seg_pose.get_result(st_img)
#     if seg_result is None or pose_result is None:
#         logger.error("==========没有检测到标准图片的seg结果或pose结果==========")
#         return None, None

#     st_pose, st_vector = compute_position.set_standard(st_img, st_depth_img, seg_result, pose_result)
#     if st_pose is None or st_vector is None:
#         logger.error("==========标准位置设定失败==========")
#         return None, None
#     else:
#         logger.info("==========标准位置设定成功==========")
#         logger.info(f"标准位置: {st_pose}")
#         logger.info(f"标准向量: {st_vector}")
#         return st_pose, st_vector

# def main():
#     """主函数 - 使用诊断处理器"""
#     seg_pose = SegPose()
#     compute_position = ComputePosition()
#     # 设置标准位置
#     st_pose, st_vector = setup_standard_position(seg_pose,compute_position)
#     if st_pose is None or st_vector is None:
#         logger.error("标准位置设定失败，程序退出")
#         return

#     # 配置参数
#     from_folder = False
#     max_queue_size = 5  # 减小队列大小以便更容易发现瓶颈
    
#     # 创建图像提供器
#     # from processing_pipeline_cl import ImageProvider  # 使用原来的ImageProvider
#     image_provider = ImageProvider(from_folder=from_folder)

#     # 创建诊断处理器 - 传入已初始化的seg_pose和compute_position实例
#     processor = DiagnosticMultiThreadProcessor(st_pose, st_vector,seg_pose, compute_position, max_queue_size)
    
#     try:
#         # 启动处理器
#         processor.start()
        
#         # 主线程负责读取图像并添加到队列
#         logger.info("开始处理图像...")
        
#         while True:
#             image_data = image_provider.get_next_image()
#             if image_data is None:
#                 logger.info("所有图像读取完成")
#                 break
#             # print(image_data)
#             success = processor.add_image(image_data)
#             if not success:
#                 time.sleep(0.01)
        
#         # 等待所有队列处理完成
#         logger.info("等待队列处理完成...")
#         while (not processor.image_queue.empty() or 
#                not processor.model_queue.empty() or 
#                not processor.position_queue.empty()):
#             time.sleep(1.0)
#             logger.info(f"等待中... 队列状态: 图像{processor.image_queue.qsize()}, 模型{processor.model_queue.qsize()}, 位置{processor.position_queue.qsize()}")
        
#         logger.info("所有任务处理完成")
        
#     except KeyboardInterrupt:
#         logger.info("收到中断信号，正在关闭...")
#     except Exception as e:
#         logger.error(f"程序错误: {e}")
#         logger.error(f"错误详情: {traceback.format_exc()}")
#     finally:
#         # 清理资源
#         processor.stop()
#         image_provider.close()
        
#         # 输出最终详细统计
#         with processor.stats_lock:
#             total_time = time.time() - processor.stats['start_time']
#             logger.info("==========程序结束==========")
#             logger.info(f"总共处理图像: {processor.stats['images_processed']}")
#             logger.info(f"总共模型处理: {processor.stats['models_processed']}")
#             logger.info(f"总共位置计算: {processor.stats['positions_processed']}")
#             logger.info(f"总共可视化: {processor.stats['visualizations_processed']}")
#             if processor.stats['images_processed'] > 0:
#                 avg_fps = processor.stats['images_processed'] / total_time
#                 logger.info(f"平均帧率: {avg_fps:.2f} FPS")
#             logger.info(f"总运行时间: {total_time:.2f} 秒")
            
#             # 输出最终性能摘要
#             perf_summary = processor.performance_monitor.get_summary()
#             logger.info("==========性能摘要==========")
#             for category, stats in perf_summary.items():
#                 if category not in ['queue_wait_times', 'errors', 'queue_drops'] and stats:
#                     logger.info(f"{category}: 平均{stats['avg']:.2f}ms, 最大{stats['max']:.2f}ms")
            
#             # 输出队列丢弃统计
#             if 'queue_drops' in perf_summary:
#                 drops = perf_summary['queue_drops']
#                 total_drops = sum(drops.values())
#                 if total_drops > 0:
#                     logger.info(f"总队列丢弃: {total_drops} (图像{drops['image']}, 模型{drops['model']}, 位置{drops['position']})")

# if __name__ == "__main__":
#     main()